#include "ActionController.h"
#define MAX_BALLOON 30

void analizeDegree(ArActionDesired *myDesired,double degree,double degreeRange);

/*
 This is the ActionTurn constructor, note the use of constructor chaining
 with the ArAction. also note how it uses setNextArgument, which makes
 it so that other things can see what parameters this action has, and
 set them.  It also initializes the classes variables.
 */
ActionController::ActionController(double stopDistance, double controlAngle,
		double velocityOnTarget, double velocityNearHurdle, double maxVelocity, ActionGo *go, ActionTurn *turn,ActionScouting *scout) :
	ArAction("Controller") {
	myStopDistance = stopDistance;
	myControlAngle = controlAngle;
	myVelocityOnTarget = velocityOnTarget;
	myGo = go;
	myTurn = turn;
	//targetold=false;
	num_target=0;
	myScout=scout;
	//oldRange=stopDistance;
	connectToCam(1);
	//myCurrentDistance=0;
	//myStartDistance=0;
	state=1;
	offset=0;
	myVelocityNearHurdle=velocityNearHurdle;
	myMaxVelocity = maxVelocity;
	target=false;
}

/*
 Sets the myRobot pointer (all setRobot overloaded functions must do this),
 finds the sonar device from the robot, and if the sonar isn't there,
 then it deactivates itself.
 */
void ActionController::setRobot(ArRobot *robot) {
	ArAction::setRobot(robot);
	mySonar = robot->findRangeDevice("sonar");
	if (mySonar == NULL) {
		ArLog::log(ArLog::Terse,
				"actionExample: ActionTurn: Warning: I found no sonar, deactivating.");
		deactivate();
	}
}

/*
 This is the guts of the Turn action.
 */
ArActionDesired *ActionController::fire(ArActionDesired currentDesired) {

	double range;
	double degree;
	Balloon targets[MAX_BALLOON];
	Balloon obstacles[MAX_BALLOON];

	int num_obst = 0;
	int near_obt = 0;
	int nearestBalloon = 0;
	double degreeRange=50;
	double degreScout=40;
	int near =0;
	bool isY = false;
	printf("----------------------GO%f\n",myGo->myMaxSpeed);
	myDesired.reset();
	// if the sonar is null we can't do anything, so deactivate
	if (mySonar == NULL) {
		deactivate();
		return NULL;
	}


	//acquisizione di un frame da CAM e controllo della presenza di obiettivi


	// return a pointer to the actionDesired, so resolver knows what to do
	// get the range of the sonar
	IplImage *src = connectToCam(0);
	range = mySonar->currentReadingPolar(-myControlAngle, myControlAngle)- myRobot->getRobotRadius();

	num_obst = getObstacles(src, obstacles, cvPoint(src->width / 2, src->height));

	 if (num_obst > 0){
	  near_obt = nearest(obstacles, num_obst);
	 }


	 bool theNearIsYellow = false;
	 if(num_obst > 0 && num_target>0){

		 Balloon nearestBalloons[2] = {targets[near],obstacles[near_obt]};
		 nearestBalloon = nearest(nearestBalloons,2);
		 if(nearestBalloon == 0){
			 theNearIsYellow = true;
		 }else{
			 theNearIsYellow = false;
		 }

	 }


	if(range>myStopDistance){
		num_target = getTargets(src, targets,cvPoint(src->width / 2, src->height));
	}else if (isYellow(src,cvPoint(src->width / 2, src->height),144,128,40)==true){
		num_target = 1;
		isY=true;
	}else{
		num_target=0;
		isY=false;
	}


	double diff = myCurrentDistance-myStartDistance;
	double diffangle = myCurrentAngle-myStartAngle;

	//if(state !=5 && range>myStopDistance && num_target==0)
	 if(range>myStopDistance && num_target==0)
	  state=1;
	 else if(range>myStopDistance && num_target!=0)
	  state=2;
	 else if(range<myStopDistance && isY==true && (num_obst==0 || theNearIsYellow == true))
	  state=3;
	 else  state=4;

/*	if(state==5 && diff > 500.0){
		state=1;
		//myStartAngle = myRobot->getOdometerDegrees();
		myRobot->resetTripOdometer();
	}
	if(state==1 && myRobot->getOdometerDistance()>= 180){
		state=5;
		myStartDistance=myRobot->getOdometerDistance();
		myCurrentDistance= myRobot->getOdometerDistance();


	}
*/


	switch (state){
	case 1: printf("Stato: Scouting\n");

			 if(target == true){
					printf("cambio-----------------------------------------<");
				  if(myTurn->myTurning > 0)
					  myScout->myDegreeStep=-degreScout;
				  else
					  myScout->myDegreeStep=degreScout;
				  target=false;
			 }

			 myGo->myMaxSpeed=myMaxVelocity;
			 if (myRobot->findAction("Go") == NULL)
				 myRobot->addAction(myGo, 60);

			 if (myRobot->findAction("Scouting") == NULL)
				 myRobot->addAction(myScout, 50);

	//		 myCurrentAngle =myRobot->getOdometerDistance();
			// myScout->myDegreeStep=20;



		    break;
	case 2:  printf("Stato: Go to Target\n");
			 myGo->myMaxSpeed=myMaxVelocity;

			 near= nearest(targets, num_target);
			 degree = targets[near].angle;

			 if (myRobot->findAction("Turn") != NULL)
			 	myRobot->remAction(myTurn);

			 if (myRobot->findAction("Scouting") != NULL)
					 myRobot->remAction(myScout);

			 if (myRobot->findAction("Go") == NULL)
				 myRobot->addAction(myGo, 50);

			 analizeDegree(&myDesired,degree,degreeRange);

		     break;
	case 3: printf("Stato: Destroy Target\n");

			if (myRobot->findAction("Turn") != NULL)
				myRobot->remAction(myTurn);
			if (myRobot->findAction("Go") != NULL)
				myRobot->remAction(myGo);
			if (myRobot->findAction("Scouting") != NULL)
				 myRobot->remAction(myScout);

			myDesired.setVel(myVelocityOnTarget);

            break;
	case 4: printf("Evita Ostacolo\n");
				if (isY==true){
						printf("------------------------------------->true\n\n");
						target = true;
				}

			   myGo->myMaxSpeed=myVelocityNearHurdle;

			   if (myRobot->findAction("Go") != NULL)
				myRobot->addAction(myGo,50);

			   if (myRobot->findAction("Scouting") != NULL)
				myRobot->remAction(myScout);

			   if (myRobot->findAction("Turn") == NULL)
				myRobot->addAction(myTurn, 60);


		    break;
/*	case 5: printf("Random Walk");

			if (myRobot->findAction("Scouting") != NULL)
				myRobot->remAction(myScout);

			if (myRobot->findAction("Go") == NULL){
				myRobot->addAction(myGo, 50);
			} else myCurrentDistance = myRobot->getOdometerDistance();

		    break;
*/
	case 6: printf("Evita Ostacolo on Target");

		    break;
	case 7: printf("Fine");
			    break;
	}

	return &myDesired;
}

void analizeDegree(ArActionDesired *myDesired,double degree,double degreeRange){

	if (degree > degreeRange) {
		myDesired->setVel(0);
		myDesired->setDeltaHeading(-10);
	} else if (degree < -degreeRange) {
				myDesired->setVel(0);
				myDesired->setDeltaHeading(10);
			} else
				myDesired->setDeltaHeading(0);

}
